﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.utils.graph;
using RobotRace.Geometry;
using RobotRace.environment.objects;
using System.Collections.ObjectModel;
using System.Collections;

namespace RobotRace.engine.path
{
    public class MultiplePathCreator : PathCreationStrategy
    {
        ArrayList wallsRect;

        public MultiplePathCreator(ReadOnlyCollection<Wall2D> walls)
            : base()
        {
            wallsRect = CollisionChecker.getRectWalls(walls);
        }

        public PathGraph createPathGraph(List<Point2D> originalCheckpoints)
        {
            PathGraph pathGraph = new PathGraph();
            if (originalCheckpoints.Count == 0)
            {
                return pathGraph;
            }


            Node prevCenterNode = null;
            Node prevLeftNode = null;
            Node prevRightNode = null;
            for (int i = 0; i < originalCheckpoints.Count; i++)
            {
                //get left and right node changerelative to center one
                Vector2D forwardV;
                if (i == 0)
                {
                    forwardV = new Vector2D(originalCheckpoints[originalCheckpoints.Count - 1], originalCheckpoints[0]);
                }
                else
                {
                    forwardV = new Vector2D(originalCheckpoints[i - 1], originalCheckpoints[i]);
                }
                //create change vectors
                Vector2D leftV = new Vector2D(forwardV.Angle + (Math.PI / 4.0));
                if (leftV.Angle < -Math.PI)
                {
                    leftV = new Vector2D(leftV.Angle + 2.0 * Math.PI);
                }
                //create points for left and right nodes
                double pointDist = forwardV.Length / 2.0;
                Point2D leftPoint = new Point2D(originalCheckpoints[i].x + (leftV.X * pointDist), originalCheckpoints[i].y + (leftV.Y * pointDist));
                Point2D rightPoint = new Point2D(originalCheckpoints[i].x - (leftV.X * pointDist), originalCheckpoints[i].y - (leftV.Y * pointDist));

                Node centerNode = new Node(originalCheckpoints[i]);
                Node leftNode = new Node(leftPoint);
                Node rightNode = new Node(rightPoint);

                bool centerAdded = addCenterNodeAndConnectIt(pathGraph, centerNode, prevCenterNode, prevLeftNode, prevRightNode);
                bool leftAdded = addLeftNodeAndConnectIt(pathGraph, leftNode, prevCenterNode, prevLeftNode, prevRightNode, centerNode);
                bool rightAdded = addRightNodeAndConnectIt(pathGraph, rightNode, prevCenterNode, prevLeftNode, prevRightNode, centerNode);

                prevCenterNode = centerNode;
                if (leftAdded)
                {
                    prevLeftNode = leftNode;
                }
                else
                {
                    prevLeftNode = null;
                }

                if (rightAdded)
                {
                    prevRightNode = rightNode;
                }
                else
                {
                    prevRightNode = null;
                }
            }
            //connect to first Node
            addCenterNodeAndConnectIt(pathGraph,pathGraph.nodes[0],prevCenterNode,prevLeftNode,prevRightNode);
            return pathGraph;
        }

        private bool addCenterNodeAndConnectIt(PathGraph pathGraph, Node centerNode, Node prevCenterNode, Node prevLeftNode, Node prevRightNode)
        {
            bool isNodeAdded = false;
            if (prevCenterNode != null)
            {
                isNodeAdded = addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevCenterNode, centerNode, 0.0);
            }
            if (prevLeftNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevLeftNode, centerNode, 1.0);
            }
            if (prevRightNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevRightNode, centerNode, 1.0);
            }
            return isNodeAdded;
        }

        private bool addLeftNodeAndConnectIt(PathGraph pathGraph, Node leftNode, Node prevCenterNode, Node prevLeftNode, Node prevRightNode, Node centerNode)
        {
            bool isNodeAdded = false;
            if (prevCenterNode != null)
            {
                isNodeAdded = addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevCenterNode, leftNode, 4.0);
            }
            if (prevLeftNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevLeftNode, leftNode, 2.0);
            }
            if (prevRightNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevRightNode, leftNode, 8.0);
            }
            if (isNodeAdded)
            {
                isNodeAdded |= addPathAndNodeIfNotAdded(isNodeAdded, pathGraph, centerNode, leftNode, 16.0);
            }
            return isNodeAdded;
        }

        private bool addRightNodeAndConnectIt(PathGraph pathGraph, Node rightNode, Node prevCenterNode, Node prevLeftNode, Node prevRightNode, Node centerNode)
        {
            bool isNodeAdded = false;
            if (prevCenterNode != null)
            {
                isNodeAdded = addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevCenterNode, rightNode, 4.0);
            }
            if (prevLeftNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevLeftNode, rightNode, 8.0);
            }
            if (prevRightNode != null)
            {
                isNodeAdded |= addPathIfNotCollidingAndNodeIfNotAdded(isNodeAdded, pathGraph, prevRightNode, rightNode, 2.0);
            }
            if (isNodeAdded)
            {
                isNodeAdded |= addPathAndNodeIfNotAdded(isNodeAdded, pathGraph, centerNode, rightNode, 16.0);
            }
            return isNodeAdded;
        }

        private bool addPathIfNotCollidingAndNodeIfNotAdded(bool isNodeAlreadyAdded, PathGraph pathGraph, Node prevNode, Node currentNode, double penalty)
        {
            if (!isPathColliding(prevNode, currentNode))
            {
                isNodeAlreadyAdded = addPathAndNodeIfNotAdded(isNodeAlreadyAdded, pathGraph, prevNode, currentNode, penalty);
            }
            return isNodeAlreadyAdded;
        }

        private bool addPathAndNodeIfNotAdded(bool isNodeAlreadyAdded, PathGraph pathGraph, Node prevNode, Node currentNode, double penalty)
        {
            if (!isNodeAlreadyAdded)
            {
                pathGraph.addNode(currentNode);
                isNodeAlreadyAdded = true;
            }
            pathGraph.addEdge(prevNode, currentNode, penalty);
            return true;
        }

        private bool isPathColliding(Node prevNode, Node currentNode)
        {
            foreach (Wall2D wall in wallsRect)
            {
                if (CollisionChecker.checkCollicionSection(prevNode.point, currentNode.point, wall.firstPoint, wall.secondPoint))
                {
                    return true;
                }
            }
            return false;
        }
    }
}
